#include <common.h>
#include <command.h>
#include <asm/io.h>
#include <linux/arm-smccc.h>
#include <serial.h>
#include "pfdi_service.h"
#include "../d2000_common.h"
#include <e_uart.h>

DECLARE_GLOBAL_DATA_PTR;

extern void d2000_system_off_entry(void);
extern void d2000_system_reset_entry(void);
extern void d2000_suspend_finish_entry(void);
extern void d2000_suspend_end_entry(void);
extern void d2000_suspend_start_entry(void);


void send_cpld_ctr(uint32_t cmd)
 {
	 uint32_t i;
	 
	 mdelay(1);
	 p_printf("u-boot : send cmd to cpld : %d \n", cmd);
	 //start
	 writel(0x1, PWR_CTR0_REG);
	 mdelay(2);
	 for(i = 0; i < cmd; i++){
		 writel(0x1, PWR_CTR1_REG);
		 mdelay(1);
		 writel(0x0, PWR_CTR1_REG);
		 mdelay(1);
	 }
	 //end
	 mdelay(2);
	 writel(0x0, PWR_CTR0_REG);

 }

static void pfdi_runtime_init(u64 gd_b)
{
 	struct global_data *gd_ptr;
 	gd_ptr = (struct global_data *)gd_b;
	memset(gd_ptr, '\0', sizeof(gd_t));
	//serial_init();
	gd->have_console = 1;

}
 void pfdi_service_system_off(u64 pfdi_mum, u64 gd_base)
{
	
	pfdi_runtime_init(gd_base);

	p_printf("u-boot : get pfdi : %lld , gd_base = 0x%llx\n", pfdi_mum, gd_base);

	send_cpld_ctr(SHUTDOWN_CPLD);
	while(1);
}

 void pfdi_service_system_reset(u64 pfdi_mum, u64 gd_base)
{


	pfdi_runtime_init(gd_base);

	p_printf("u-boot : get pfdi : %lld , gd_base = 0x%llx\n", pfdi_mum, gd_base);
	
	send_cpld_ctr(REBOOT_CPLD);
	while(1);
}

 void pfdi_service_suspend_finish(u64 pfdi_mum, u64 gd_base)
{
	u_register_t k;
	struct arm_smccc_res res;

	pfdi_runtime_init(gd_base);

	p_printf("u-boot : get pfdi : %lld , gd_base = 0x%llx\n", pfdi_mum, gd_base);
	
	arm_smccc_smc(PFDI_DONE, (uint64_t)&k, 0, 0, 0, 0, 0, 0, &res);
	while(1);
}
 void pfdi_service_suspend_end(u64 pfdi_mum, u64 gd_base)
{
	u_register_t k;
	struct arm_smccc_res res;

	pfdi_runtime_init(gd_base);

	p_printf("u-boot : get pfdi : %lld , gd_base = 0x%llx\n", pfdi_mum, gd_base);
	
	arm_smccc_smc(PFDI_DONE, (uint64_t)&k, 0, 0, 0, 0, 0, 0, &res);
	while(1);
}


 void pfdi_service_suspend_start(u64 pfdi_mum, u64 gd_base)
 {
	pfdi_runtime_init(gd_base);

	p_printf("u-boot : get pfdi : %lld , gd_base = 0x%llx\n", pfdi_mum, gd_base);
	send_cpld_ctr(S3_SETUP_CPLD);
	 while(1);
 }


void register_pfdi(void)
{
	pfdi_vectors_t pfdi_svc;
	
	struct arm_smccc_res res;
	memset(&pfdi_svc, 0, sizeof(pfdi_vectors_t));
	
	pfdi_svc.system_off_entry = (u_register_t)d2000_system_off_entry;
	pfdi_svc.system_reset_entry = (u_register_t)d2000_system_reset_entry;
	pfdi_svc.suspend_finish_entry = (u_register_t)d2000_suspend_finish_entry;	
	pfdi_svc.suspend_end_entry = (u_register_t)d2000_suspend_end_entry;
	pfdi_svc.suspend_start_entry = (u_register_t)d2000_suspend_start_entry;

	printf("system_off_entry addr =0x%lx\n", pfdi_svc.system_off_entry);
	printf("system_reset_entry addr =0x%lx\n", pfdi_svc.system_reset_entry);
	printf("suspend_entry addr =0x%lx\n", pfdi_svc.suspend_start_entry);	
	printf("suspend_end_entry addr =0x%lx\n", pfdi_svc.suspend_end_entry);
	printf("suspend_finish_entry addr =0x%lx\n", pfdi_svc.suspend_finish_entry);
	dsb();
	isb();
	flush_dcache_range((unsigned long)&pfdi_svc, sizeof(pfdi_vectors_t));
	arm_smccc_smc(PFDI_REGISTER, (uint64_t)&pfdi_svc, 0, 0, 0, 0, 0, 0, &res);
}



